import rosbag
from colorama import Fore, Style
import os
import sys
import warnings
import def_function

# 获取当前脚本的完整路径
script_path = os.path.abspath(sys.argv[0])
# 从完整路径中获取目录
script_dir = os.path.dirname(script_path)
# 从完整路径中分离出文件名
script_name = os.path.basename(script_path)
# 使用 splitext() 函数分离文件名和扩展名
script_name_without_extension, _ = os.path.splitext(script_name)

print(Fore.YELLOW+f"python script name:", sys.argv[0])
if os.getenv('DEBUG') == 'true':
    python_param = '/mnt/01_code/python/rosbag_analysis/dataspace/ROCKY/PL077159_recording_dfdi_console_manual_recording_20230904-104320_20230904-104346_0_filter.bag'
else:
    if len(sys.argv)!=2:
        print(Fore.RED+f"you need python params!")
        sys.exit()
    python_param = script_dir + "/" + sys.argv[1]
    if os.path.isabs(sys.argv[1]):
        python_param = sys.argv[1]
rosbag_data = rosbag.Bag(python_param)

# 获取类型和主题信息
# rosbag_info = rosbag_data.get_type_and_topic_info()
# info.topics 是一个字典，它的键是 topic 的名字
# all_topics = list(rosbag_info.topics.keys())
topics = ['/vehicle/body_report', '/vehicle/chassis_report']
speed_on_dashboard = 0.0
speed_average = 0.0
warning_msg = ""
error_msg = ""
arry_error_info = list()
dict_error_info = dict()
judge = def_function.judge_value.ValueJudge()
# https://baijiahao.baidu.com/s?id=1738344943951285469&wfr=spider&for=pc
for msg_topic, msg, t in rosbag_data.read_messages(topics=topics):
    if msg_topic == '/vehicle/body_report':
        speed_on_dashboard = msg.auxiliary_report.auxiliary_report_data.vehicle_speed_on_dashboard
    if msg_topic == '/vehicle/chassis_report':
        speed_average = msg.throttle_info_report.throttle_info_report_data.vehicle_speed_average * 3.6

    error_type = "表显车速不和符合国标"
    judge_condition = "不满足0≤指示车速表-实际车速表≤实际车速/10+4 KM/H"
    result = False
    if (0 < speed_average) and (0 < speed_on_dashboard):
        result = (0 > (speed_on_dashboard - speed_average)) or ((speed_average / 10 + 4) < (speed_on_dashboard - speed_average))
    message = "[" + error_type + "{" + judge_condition + "}]" + "表显车速[" + str(speed_on_dashboard) + "], 实际车速[" + str(speed_average)+ "]"
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

    error_type = "表显车速信号为0"
    keep_time = 10
    judge_condition = "实际车速表大于0时候表显车速为0持续时间" + str(keep_time) + "秒"
    result = judge.timeout(t.to_sec(), 0 < speed_average and 0 == speed_on_dashboard, True, keep_time, "==")
    message = "[" + error_type + "{" + judge_condition + "}]" + "表显车速[" + str(speed_on_dashboard) + "], 实际车速[" + str(speed_average)+ "]"
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type, arry_error_info)

################################################################################################
rosbag_data.close()
list_print = list()
for value in dict_error_info.values():
    if (0 != value.start_time and 0 == value.end_time):
        list_print.append("[" + str(value.start_time) + "-" + "bag_end]" + value.message)
for item in arry_error_info:
    list_print.append("[" + str(item.start_time) + "-" + str(item.end_time) + "@diff:" + "{:.3f}".format(item.end_time - item.start_time) + "]" + item.message)
if list_print:
    print(Fore.RED+f"", script_name_without_extension)
for item in list_print:
    print(Fore.CYAN+f"", item)